#include "math_.h"


/**
 * 三维极坐标转直角坐标
 * 极坐标：(r, theta, phi) -> 直角坐标：(x, y, z)
 * theta：方位角（xy平面），phi：俯仰角（xz平面）
 */
void polar3d_to_cartesian(const RadarPoint3D *point, double *x, double *y, double *z)
{
    *x = point->r * sin(point->phi) * cos(point->theta); // x = r·sinφ·cosθ
    *y = point->r * sin(point->phi) * sin(point->theta); // y = r·sinφ·sinθ
    *z = point->r * cos(point->phi);                     // z = r·cosφ
}

/**
 * 三维直角坐标转极坐标
 * 直角坐标：(x, y, z) -> 极坐标：(r, theta, phi)
 */
void cartesian_to_polar3d(double x, double y, double z, double *r, double *theta, double *phi)
{
    *r = sqrt(x * x + y * y + z * z);     // 距离
    *theta = atan2(y, x);                 // 方位角（范围：[-π, π]）
    *phi = atan2(sqrt(x * x + y * y), z); // 俯仰角（范围：[0, π]）
}

/**
 * 3x3矩阵乘法：C = A * B
 */
void mat33_mult(const double A[3][3], const double B[3][3], double C[3][3])
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            C[i][j] = 0;
            for (int k = 0; k < 3; k++)
            {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

/**
 * 6x6矩阵乘法：C = A * B
 */
void mat66_mult(const double A[6][6], const double B[6][6], double C[6][6])
{
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            C[i][j] = 0;
            for (int k = 0; k < 6; k++)
            {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

/**
 * 6x6矩阵转置：B = A^T
 */
void mat66_transpose(const double A[6][6], double B[6][6])
{
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            B[i][j] = A[j][i];
        }
    }
}

/**
 * 3x3矩阵求逆（针对对称矩阵，简化计算）
 * 返回值：true=成功，false=失败（行列式接近0）
 */
bool mat33_inv(const double A[3][3], double invA[3][3])
{
    // 计算行列式
    double det = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) - A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]);

    if (fabs(det) < 1e-8)
        return false; // 行列式接近0，不可逆
    double inv_det = 1.0 / det;

    // 伴随矩阵 / 行列式（对称矩阵优化）
    invA[0][0] = (A[1][1] * A[2][2] - A[1][2] * A[2][1]) * inv_det;
    invA[0][1] = (A[0][2] * A[2][1] - A[0][1] * A[2][2]) * inv_det;
    invA[0][2] = (A[0][1] * A[1][2] - A[0][2] * A[1][1]) * inv_det;
    invA[1][0] = invA[0][1]; // 对称矩阵特性
    invA[1][1] = (A[0][0] * A[2][2] - A[0][2] * A[2][0]) * inv_det;
    invA[1][2] = (A[0][2] * A[1][0] - A[0][0] * A[1][2]) * inv_det;
    invA[2][0] = invA[0][2]; // 对称矩阵特性
    invA[2][1] = invA[1][2]; // 对称矩阵特性
    invA[2][2] = (A[0][0] * A[1][1] - A[0][1] * A[1][0]) * inv_det;

    return true;
}